/**
 * @file Implicit_LMM.h
 * @brief The class for implicit lmm.
 * @author XDDDD
 * @version 
 * @date 2021-04-28
 */

#ifndef __PAISLEYPARK__IMPLICIT_LMM_H__
#define __PAISLEYPARK__IMPLICIT_LMM_H__

#include "LMM.h"
#include "Explicit_LMM.h"
#define epsilon 0.000000000001

template <std::size_t DIM>
class Implicit_LMM : public LMM<DIM> {
public:
	/**
	 * @brief The implementation of one step method by implicit lmm. 
	 *
	 * @param _Pnts The vector of initial points.
	 * @param _stepl The given length of step.
	 * @param _f The given vector functions.
	 */
	void one_step_method(std::vector<Point<DIM> >& _Pnts, const double _stepl, VectorFunction<DIM, DIM>& _f) const override;

};

template <std::size_t DIM>
void Implicit_LMM<DIM>::one_step_method(std::vector<Point<DIM> >& _Pnts, const double _stepl, VectorFunction<DIM, DIM>& _f) const {
	std::size_t s = this -> get_s();
	std::size_t order = this -> get_p();
	std::size_t num_init = this -> get_num_init();
	if(s == 0) {
			std::cerr << "Unset the coefficients." << std::endl;
			std::exit(-1);
	}
	if(_Pnts.size() == 1 && _Pnts[0].get_time() == 0 && num_init > 1) {
		this -> starting_method(_Pnts, _stepl, num_init, _f);
	}
	if(_Pnts.size() < s) {
		std::cerr << "Do not have enough initial points." << std::endl;
		std::exit(-1);
	}
	
	Point<DIM> p;
	for(int i = 0; i != s; i++) {
		p = p + _stepl*(this -> get_beta_i(s - 1 - i))*_f(_Pnts[num_init - 1 - i]) 
			  - (this -> get_alpha_i(s - 1 - i))*_Pnts[num_init - 1 - i]; //Constent part.
	}
	//std::cout << "P:" << p << std::endl;
	Point<DIM> dfx_k, h_k, fx_k, x_k;
	Point<DIM> x, Fx;
	Eigen::MatrixXd Inverse_DF_k(DIM , DIM);
	Eigen::VectorXd delta_x_k(DIM);
	double norm;
	int n_step = 0;
	double h = 0.0001;
	double gamma = 0.1;
	bool stop_Newton = false;
	double delta[DIM] = {0};
	double omega = 1;

	//Use ABM to predict p_s.
	Point<DIM> Un = _Pnts[0];					
	int pole;	
	Explicit_LMM<DIM> Predict_Method;
	Predict_Method.set_cof("ABM_Info", order);
	Predict_Method.one_step_method(_Pnts, _stepl, _f);
	_Pnts.insert(_Pnts.begin(), Un);
	Un = *(_Pnts.end() - 1);
	_Pnts.pop_back();
	x_k = Un;
	while (n_step < 10 && !stop_Newton) {		//This modified Newton's method has order 2,
												//to get the accuracy of epsilon it may iterate 8 times.
		h = h*gamma;							//h=0.5^(n_step+1).
		fx_k = _f(x_k);							//fx_k = f(x_k).
		for(int j = 0; j != DIM; j++) {
			if(j != 0) {
				delta[j - 1] = 0;
			}
			delta[j] = h;
			h_k.set_point(delta);
			h_k = h_k + x_k;
			dfx_k = _stepl*(this -> get_beta_i(s))*(1.0/h)*(_f(h_k) - fx_k);
			for(int i = 0; i != DIM; i++) {
				Inverse_DF_k(i, j) = dfx_k[i];
				if(i == j)
					Inverse_DF_k(i, j) -= (this -> get_alpha_i(s));		//Aij=dFi/dxj.	
				if(abs(Inverse_DF_k(i, j)) < epsilon) {					//Omit small part.
					Inverse_DF_k(i, j) = 0;
				}
			}
			delta_x_k(j) = -p[j] - _stepl*(this -> get_beta_i(s))*fx_k[j] 
						+ (this -> get_alpha_i(s))*x_k[j];					//Dfx_k*x_k=-Fx_k.
			if(abs(delta_x_k(j)) < epsilon) {							//Omit small parts.
				delta_x_k(j) = 0;
			}
		}

		delta_x_k = Inverse_DF_k.colPivHouseholderQr().solve(delta_x_k);
		
		pole = 0;
		for(int i = 0; i != DIM; i++) {
			if(abs(delta_x_k(i)) < epsilon) {
				delta_x_k(i) = 0;
				pole++;
			}
			delta[i] = delta_x_k(i);
		}
		if(pole == DIM) {
			stop_Newton = true;
		}
		Fx = _stepl*(this -> get_beta_i(s))*fx_k - (this -> get_alpha_i(s))*x_k + p;
		norm = Fx.norm_2();
		x.set_point(delta);
		omega = 1.0;
		x_k = x_k + omega*x;
		Fx = _stepl*(this -> get_beta_i(s))*_f(x_k) - (this -> get_alpha_i(s))*x_k + p;
		pole = 0;
		while(Fx.norm_2() >= norm && pole < 10) {					//ensure ||Fx_k+1|| < ||Fx_k||
			omega = omega*0.5;
			x_k = x_k - omega*x;
			Fx = _stepl*(this -> get_beta_i(s))*_f(x_k) - (this -> get_alpha_i(s))*x_k + p;	
			pole++;
		}
		if(Fx.norm_2() < epsilon) {
			stop_Newton = true;
		}
		else if(Fx.norm_2() > norm/2) {
			h = h*gamma;
		}
		for(int i = 0; i != DIM; i++) {
			delta[i] = 0;
		}
		n_step++;
	}
	x_k.set_time(_Pnts[num_init - 1].get_time() + _stepl);
	_Pnts.erase(_Pnts.begin());
	_Pnts.insert(_Pnts.begin() + num_init - 1, x_k);
};

#else
//Do nothing.
#endif
